#include "ros/ros.h"

#include "nidaqIO/Doubles.h" 

#include <sstream>
#include <vector>
#include <iostream>
#include <Mmsystem.h>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>

#include "daqAdapter.h"

DaqIO *daqWrite=NULL, *daqRead=NULL;

double *writeBuffer=NULL;

boost::mutex writeMutex;

bool isnan(double x)
{
	return x!=x;
}
	
void chatterCallback(const nidaqIO::Doubles::ConstPtr& msg)
{
	boost::mutex::scoped_lock lock(writeMutex) ;
	for (int i=0;i<daqWrite->numChan;i++)
	{
		if(!isnan(msg->data[i]))
			writeBuffer[i] = msg->data[i];
	}

#if 0
	// very slow
	std::stringstream ss;
	ss << *msg;
	ROS_INFO("I heard: \n[%s]", ss.str().c_str());
#else
	ROS_INFO("Writing :[%.3f %.3f %.3f ...]", msg->data[0], msg->data[1], msg->data[2]);
#endif
	daqWrite->write(writeBuffer);
}

bool rosLoop(int argc, char **argv)
{
	ros::init(argc, argv, "nidaqIO");

	ros::NodeHandle n;

	int queueLength=1;
	ros::Publisher chatter_pub = n.advertise<nidaqIO::Doubles>("diego/readraw", queueLength);

	ros::Subscriber sub = n.subscribe("diego/writeraw", queueLength, chatterCallback, ros::TransportHints().tcpNoDelay());

	std::cout<< "Streaming " << daqRead->numChan << " read and "<< daqWrite->numChan <<" write channels" <<std::endl;
	/*
	while(1)
	{
		ros::Time t= ros::Time::now();
		Sleep(10);
		printf("%ld,%ld\n",t.sec, t.nsec);
	}*/
	
	int count=0;

	ros::AsyncSpinner s(2);
	s.start();

	timeBeginPeriod(1); //start a 1ms timer

	double *readBuffer = new double[daqRead->numChan];
	while (ros::ok())
	{
		count++;

		daqRead->read(readBuffer);
		nidaqIO::Doubles msg;
		msg.data=std::vector<double>(readBuffer, readBuffer+daqRead->numChan);
		msg.header.seq = count;

		msg.header.stamp.fromNSec(1e6*timeGetTime());

		chatter_pub.publish(msg);
	}
	timeEndPeriod(1);	
	
	delete[] readBuffer;
	return 0;
}


int main(int argc, char **argv)
{
	const int rate=100;
	std::vector<std::string> inChan;
	inChan.push_back("PXI1Slot2/ai0:61"); //PS01-PS62 
	inChan.push_back("PXI1Slot3/ai52:77,PXI1Slot3/ai0:37"); //PS63-PS88, POT01-POT38
	daqRead=new DaqGroupIO(inChan,rate,-1,6);

	std::vector<std::string> outChan;
	outChan.push_back("PXI1Slot4/ao0:31"); //VALVE 1-32
	outChan.push_back("PXI1Slot5/ao0:31"); //VALVE 33-64
	outChan.push_back("PXI1Slot6/ao0:11,PXI1Slot6/ao12:23"); //VALVE 65-76, VALVE77-88 (hand)
	daqWrite = new DaqGroupIO(outChan,rate,2,8);
	writeBuffer = new double[daqWrite->numChan];
	for(int i=0;i<daqWrite->numChan;i++)
		writeBuffer[i]=3;
	daqWrite->write(writeBuffer);

	std::cerr << "Connecting to ROS_MASTER\n";
	if(rosLoop(argc, argv))
	{
		std::cerr << "Normal termination\n";
	}
	else
	{
		std::cerr<<"Lost connection to ROS_MASTER, quit\n";
	}

	delete[] writeBuffer;
	return 0;
}
